#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/distances.h>

int main(int argc, char** argv)
{
    // 加载PCD文件
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("./ransac_data/line.pcd", *cloud) == -1)
    {
        PCL_ERROR("Couldn't read file\n");
        return -1;
    }
    std::cout <<"points size:"<<cloud->points.size() << std::endl;
    // 选择要计算距离的两个点的索引
    
    int index1 = 0; // 第一个点的索引
    int index2 = 1; // 第二个点的索引
    cout << "input two points index ";
    std::cin>>index1>>index2;
    // 计算点与点之间的距离
    float distance = pcl::euclideanDistance(cloud->points[index1], cloud->points[index2]);
    std::cout << "Distance between point " << index1 << " and point " << index2 << ": " << distance << std::endl;

    // 初始化PCL Visualizer
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Point Cloud Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");

    // 在可视化窗口中显示点与点之间的距离
    pcl::PointXYZ p1 = cloud->points[index1];
    pcl::PointXYZ p2 = cloud->points[index2];
    viewer->addLine<pcl::PointXYZ>(p1, p2, 255, 0, 0, "line");

    // 循环显示可视化窗口，直到窗口被关闭
    while (!viewer->wasStopped())
    {
        viewer->spinOnce();
    }

    return 0;
}
